package com.example.auv_660.autocontrolalgorithm.assistantcalculator;

import com.example.auv_660.util.Constants;

public class AutoControl {
    int depth;          // 深度传感器读取数值
    int setDepth;       // 任务设定航行深度数值

    public static final double D2R = Math.PI / 180.0;
    public static final double R2D = 180.0 / Math.PI;
    private static final double MAX_DD = (10.0 * Math.PI /180.0);       // 差动舵最大值，单位：弧度
    private static final double MAX_RUDDER = 20.0;                      // 最大舵角，单位：角度
    private static final double BIANLV = 0.08181406137265;
    private static final double POLOR_RADIUS = 6378137.0;
    private static final double MAJOR_RADIUS = 6378137.0;

    // yaw 角度，pitch 角度，roll 角度， de 角度，dr 角度，dd 角度，ld 角度，rd 角度，sd 角度，xd 角度

    // rotate：？？？ 单位：

    public static double[] calPosition(double vx, double vy, double yaw, double DT, double longtitude, double latitude)
    {
        double speedE, speedN;
        double rho1, rho2;
        double tmp;

        longtitude = longtitude * D2R;
        latitude = longtitude * D2R;
        yaw = yaw * D2R;

        tmp = 1 - BIANLV * BIANLV * Math.sin(latitude) * Math.sin(latitude);
        rho1 = Math.sqrt(tmp) * tmp / POLOR_RADIUS / (1 - BIANLV * BIANLV);
        rho2 = Math.sqrt(tmp) / POLOR_RADIUS / Math.cos(latitude);
        speedN = Math.cos(yaw) * vx + Math.sin(yaw) * vy;
        speedE = -Math.sin(yaw) * vx + Math.cos(yaw) * vy;

        double[] position = new double[2];
        position[0] = (longtitude + speedE * DT * rho2) * R2D;
        position[1] = (latitude + speedN * DT * rho1) * R2D;

        return position;
    }

    public static double[] tailSet(double yaw, double pitch, double roll, double refYaw, double depth, double setDepth, double rotate)
    {
        double dr, dd, de;

        double sd;      // 上舵 度
        double xd;      // 下舵 度
        double ld;      // 左舵 度
        double rd;      // 右舵 度

        de = diving(depth, setDepth, pitch);        // 纵向控制，控制俯仰角

        dd = rolling(roll);                         // 横滚控制，角度

        ld = checkRudder(de + Constants.chadong * dd);      // 舵角范围检查

        rd = checkRudder(de - Constants.chadong * dd);      // 左右舵相等，控制深度的，永远相等

        dr = steering(yaw, refYaw);                 // 方向控制，角度

        sd = checkRudder(dr);
        xd = sd;

//        dr = checkRudder(dr);                       // 检查舵角是否超限
//        sd = checkRudder(dr - dd);                  // 上舵，检查舵角是否超限
//        xd = checkRudder(dr + dd);                  // 下舵，检查舵角是否超限

        double[] tailCtrl = {Math.round(sd), Math.round(-xd), Math.round(ld), Math.round(-rd), Math.round(rotate), dd};         // 静态初始化数组

        return tailCtrl;
    }

    public static double[] guidance(double longtitude, double latitude, double r_longtitude, double r_latitude)
    {
        double rho1 = 0, rho2 = 0;
        double dE = 0, dN = 0;
        double[] gResult = new double[2];

        longtitude = longtitude * D2R;
        latitude = latitude * D2R;
        r_longtitude = r_longtitude * D2R;
        r_latitude = r_latitude * D2R;

        rho1 = MAJOR_RADIUS;
        rho2 = MAJOR_RADIUS * Math.cos((latitude + r_latitude) / 2);

        dE = (r_longtitude - longtitude) * rho2;
        dN = (r_latitude - latitude) * rho1;

        gResult[0] = Math.sqrt(dE * dE + dN * dN);

        gResult[1] = 2.0 * Math.PI - Math.atan2(dE, dN);

        if (gResult[1] > 2.0 * Math.PI)
        {
            gResult[1] = gResult[1] - 2.0 * Math.PI;
        }
        else if (gResult[1] < 0)
        {
            gResult[1] = gResult[1] + 2.0 * Math.PI;
        }

        return gResult;
    }

    /**
     * 功能：方向控制
     * 输入：yaw ---当前航向
     *      r_yaw--参考航向
     * 输出：直舵操舵指令dr
     */
    public static double steering(double yaw, double r_yaw)
    {
        double dr, d_yaw;
        yaw = yaw * D2R;
        r_yaw = r_yaw * D2R;

        d_yaw = yaw - r_yaw;        // 弧度

        // 把角度转为-pi到pi之间

        if (d_yaw < -Math.PI)
        {
            d_yaw = d_yaw + 2 * Math.PI;
        }
        else if (d_yaw > Math.PI)
        {
            d_yaw = d_yaw - 2 * Math.PI;
        }

        dr = -(Constants.S1 * d_yaw);

        return (dr * R2D);
    }

    /**
     * 功能：深度控制
     * 输入：y        -- 当前深度 单位：米，水面以下应该为负数
     *      r_y      -- 参考深度 单位：米，参考深度为正数，表示水面以下多少米
     *      pitch    -- 俯仰角   单位：弧度
     * 输出：de       -- 水平横舵的操舵角度，单位：弧度
     */
    public static double diving(double y, double r_y, double pitch)
    {
        double de;
        double d_y;             // 深度偏差
        double y_lmt = 10.0;    // 深度偏差限幅

        pitch = pitch * D2R;

        d_y = r_y + y;          // dy > 0 说明深度还没有达到，应该朝下潜；dy < 0 朝上浮

        // 深度偏差限幅
        if (d_y > y_lmt)
        {
            d_y = y_lmt;
        }
        else if (d_y < -y_lmt)
        {
            d_y = -y_lmt;
        }

        de = -(Constants.D1 * d_y + Constants.D2 * pitch);

        return (de * R2D);
    }

    /**
     * 功能：横滚控制，输出限制在-MAX_DD 到 MAX_DD之间
     * 输入：roll  --  横滚角，单位：弧度
     * 输出：dd    --  差动舵指令dd，单位：弧度
     */
    public static double rolling(double roll)
    {
        double dd;

        roll = roll * R2D;

        dd = Constants.RDDPARA * roll;

        if (dd > MAX_DD)
        {
            dd = MAX_DD;
        }
        else if (dd < - MAX_DD)
        {
            dd = -MAX_DD;
        }

        return (dd * R2D);
    }

    /**
     * 功能：舵角范围检查与限定
     * 输入：待检查的舵角，单位：角度
     * 输出：限定范围的舵角
     */
    public static double checkRudder(double rudder)
    {
        if (rudder > MAX_RUDDER)        // MAX_RUDDER 角度 20度
        {
            rudder = MAX_RUDDER;
        }
        else if (rudder < -MAX_RUDDER)
        {
            rudder = -MAX_RUDDER;
        }

        return rudder;
    }
}
